//
// Created by kevin on 2020/7/13.
//

#ifndef IMAGE_BOX_IMAGE_H
#define IMAGE_BOX_IMAGE_H
#include "image_box/common_include.h"

namespace wtr {

    class Node {
    private:
        ros::NodeHandle nh_;                                 //实例化一个节点
        cv::Mat left_, right_;
        image_transport::ImageTransport it_;

//        image_transport::Subscriber image_sub_left_;         //订阅节点
//        image_transport::Subscriber image_sub_right_;        //订阅节点
        image_transport::Publisher image_pub_raw_, image_pub_box_;           //发布节点
        ros::Publisher pub_pc;
        cv_bridge::CvImage cv_ptr_img;  //申明一个CvImagePtr
        ros::Publisher start_point_pub;
        ros::Subscriber potSub;
//        float x, y;
        float pos[2];
    public:
        void posCallback(const std_msgs::Float32MultiArray::ConstPtr &msg) {
            pos[0] = msg->data[0];
            pos[1] = msg->data[1];
//            std::cout<<x<<" "<<y<<std::endl;
        }
        Node()
                : it_(nh_) {
            potSub = nh_.subscribe<std_msgs::Float32MultiArray>("/uav/load/raw", 1, &wtr::Node::posCallback, this);
//            sub = nh.subscribe<sensor_msgs::Joy>("joy",10,&Xbox::JoyCallback,this);
            start_point_pub = nh_.advertise<std_msgs::UInt8MultiArray>("/uav/image/pos", 1);
            image_pub_box_ = it_.advertise("/uav/image/box", 1);
            image_pub_raw_ = it_.advertise("/uav/image/raw", 1);
//            pub_pc = nh_.advertise<sensor_msgs::PointCloud2>("/bi2pc/camera/PointCloud", 10);
            cv_ptr_img.header.frame_id = "image";
            cv_ptr_img.encoding = "bgr8";
        }
        ~Node() {};


        float *getLoadPos() {
            return (float * )pos;
        }


        void imageBoxPublish(cv::Mat img) {
            img.convertTo(img, CV_8UC1);
            ros::Time time = ros::Time::now();
            cv_ptr_img.header.stamp = time;
            cv_ptr_img.image = img;
            cv_ptr_img.encoding = "mono8";
            image_pub_box_.publish(cv_ptr_img.toImageMsg());
//        ROS_INFO("Converted Successfully!");
        }

        void imageRawPublish(cv::Mat img) {

            img.convertTo(img, CV_8UC1);
            ros::Time time = ros::Time::now();
            cv_ptr_img.header.stamp = time;
            cv_ptr_img.image = img;
            cv_ptr_img.encoding = "bgr8";
            image_pub_raw_.publish(cv_ptr_img.toImageMsg());
//        ROS_INFO("Converted Successfully!");
        }

        void imagePosPublish(uint8_t *pos) {
            std_msgs::UInt8MultiArray pos_pub;
            pos_pub.data.resize(2);
            pos_pub.data[0] = pos[0];
            pos_pub.data[1] = pos[1];
            start_point_pub.publish(pos_pub);
        }

    };

    class imageBox {

        cv::VideoCapture capture;
        cv::Mat frameRaw, gray;
        float x_, y_;
        int boxSize;
        cv::Mat mask;
    public:
        cv::Mat getMask(){
            return mask;
        }
        cv::Mat getRaw(){
            return frameRaw;
        }
        imageBox() {
            boxSize = 120;
            capture.open(0);
            cv::namedWindow("RAW");
        }

        void readImage() {
            if (capture.isOpened()) {
                capture.read(frameRaw);
                cvtColor(frameRaw, gray, CV_BGR2GRAY);
                imshow("RAW", frameRaw);
                cv::waitKey(1);
            }
        }

        void setLoadPos(float* pos) {
//            std::cout<<pos[0]<<pos[1]<<std::endl;
            x_ = pos[0];
            y_ = pos[1];


        }

        void setBox() {
            int boxX = (int) (x_ * gray.rows / 4096);
            int boxY = (int) ((4096-y_) * gray.cols / 4096);
            mask = gray;
            //遍历图像的每个像素
            std::cout<<x_<<" "<<y_<<std::endl;
            for (int j = 0; j < gray.rows; ++j) {
                uchar *data = mask.ptr<uchar>(j);
                for (int i = 0; i <gray.cols ; ++i) {
                    if (j < boxX || j> (boxX + boxSize)
                        || i< boxY || i > (boxY + boxSize)
                            )
                        data[i] = 0;
                }
            }
            cv::imshow("Box", mask);
            cv::waitKey(1);
        }


    };



}

#endif //IMAGE_BOX_IMAGE_H
